using System.Diagnostics;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.CollisionDetection
{
    /// <summary>
    /// 有关非凸可碰撞对中单个接触的信息。
    /// 由于不能保证接触面是平面,因此非凸对在每个接触点处可以有不同的表面底面。
    /// </summary>
    [StructLayout(LayoutKind.Explicit, Size = 32)]
    public struct NonconvexContact
    {
        /// <summary>
        /// 从可碰撞A的位置到接触位置的偏移。
        /// </summary>
        [FieldOffset(0)]
        public Vector3 Offset;
        /// <summary>
        /// 在此接触处两个可碰撞物体之间的穿透深度。负值表示分隔。
        /// </summary>
        [FieldOffset(12)]
        public float Depth;
        /// <summary>
        /// 接触的曲面基础。如果转换为旋转矩阵,则X和Z表示切线方向,Y表示接触法线。从可碰撞B到可碰撞A的点。
        /// </summary>
        [FieldOffset(16)]
        public Vector3 Normal;
        /// <summary>
        /// 生成此联系人的冲突中涉及的要素的ID。如果联系人的特征id与上一帧中的特征id相同,则表示
        /// 形状的相同部分促成了它的创造。这对于在帧与帧之间传送信息非常有用。
        /// </summary>
        [FieldOffset(28)]
        public int FeatureId;
    }
    /// <summary>
    /// 有关凸可碰撞对中单个接触的信息。凸面可碰撞对在流形上共享一个曲面基础,因为接触面被保证为平面。
    /// </summary>    
    [StructLayout(LayoutKind.Explicit, Size = 20)]
    public struct ConvexContact
    {
        /// <summary>
        /// 从可碰撞A的位置到接触位置的偏移。
        /// </summary>
        [FieldOffset(0)]
        public Vector3 Offset;
        /// <summary>
        /// 在此接触处两个可碰撞物体之间的穿透深度。负值表示分隔。
        /// </summary>
        [FieldOffset(12)]
        public float Depth;
        /// <summary>
        /// 生成此联系人的冲突中涉及的要素的ID。如果联系人的特征id与上一帧中的特征id相同,则表示
        /// 形状的相同部分促成了它的创造。这对于在帧与帧之间传送信息非常有用。
        /// </summary>
        [FieldOffset(16)]
        public int FeatureId;
    }

    public interface IContactManifold<TManifold> where TManifold : struct, IContactManifold<TManifold>
    {
        /// <summary>
        /// 获取歧管中的触点数。
        /// </summary>
        int Count { get; }

        /// <summary>
        /// 获取接触流形是否由一对凸起对象创建。如果是凸的,则为true;如果不是凸的,则为false。
        /// </summary>
        bool Convex { get; }

        /// <summary>
        /// 检索与请求的联系人关联的功能ID。
        /// </summary>
        /// <param name="contactIndex">要获取其功能ID的联系人的索引。</param>
        /// <return>请求的联系人的功能ID。</return>
        int GetFeatureId(int contactIndex);

        /// <summary>
        /// 检索联系人数据的副本。
        /// </summary>
        /// <param name="contactIndex">要从中复制数据的联系人的索引。</param>
        /// <param name="Offset">从第一个可碰撞对象的位置到接触位置的偏移量。</param>
        /// 请求接触处的接触面的<param name="Normal">法线。从可碰撞B到可碰撞A的点</param>
        /// <param name="Depth">请求联系人的穿透深度。</param>
        /// 请求的联系人的<param name="feature ureId">功能ID。
        /// 功能ID表示可碰撞对象的哪些部分形成了联系人,并可用于跨帧跟踪唯一联系人。</param>
        void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId);

        // 不能将引用返回到This实例,但是有参数的引用返回很方便,并且接口不需要静电函数,因此.
        /// <summary>
        /// 拉出对接触深度的引用。
        /// </summary>
        /// <param name="Manifold">要从中提取引用的流形。</param>
        /// <param name="contactIndex">要从中提取数据的联系人。</param>
        /// <return>对联系人深度的引用。</return>
        ref float GetDepth(ref TManifold manifold, int contactIndex);

        /// <summary>
        /// 拉出对联系人正常状态的引用。从可碰撞B到可碰撞A的点。对于共享法线的凸流形,所有接触指数将仅返回对流形宽度法线的引用。
        /// </summary>
        /// <param name="Manifold">要从中提取引用的流形。</param>
        /// <param name="contactIndex">要从中提取数据的联系人。</param>
        /// <return>对接触法线(或凸流形中的流形宽度法线)的引用。</return>
        ref Vector3 GetNormal(ref TManifold manifold, int contactIndex);

        /// <summary>
        /// 拉出对接触偏移量的引用。
        /// </summary>
        /// <param name="Manifold">要从中提取引用的流形。</param>
        /// <param name="contactIndex">要从中提取数据的联系人。</param>
        /// <return>对联系人偏移量的引用。</return>
        ref Vector3 GetOffset(ref TManifold manifold, int contactIndex);

        /// <summary>
        /// 拉出对联系人功能ID的引用。
        /// </summary>
        /// <param name="Manifold">要从中提取引用的流形。</param>
        /// <param name="contactIndex">要从中提取数据的联系人。</param>
        /// <return>对联系人功能ID的引用。</return>
        ref int GetFeatureId(ref TManifold manifold, int contactIndex);

    }

    // TODO：如果L2实际上不能容纳所有内容,我们可以使用专用的存储类型来处理诸如延续之类的事情。看起来不太可能,但如果需要也没那么难。

    /// <summary>
    /// 包含与非凸接触流形关联的数据。
    /// </summary>
    [StructLayout(LayoutKind.Explicit, Size = 144)]
    public unsafe struct NonconvexContactManifold : IContactManifold<NonconvexContactManifold>
    {
        /// <summary>
        /// 从可碰撞A到可碰撞B的偏移。
        /// </summary>
        [FieldOffset(0)]
        public Vector3 OffsetB;
        [FieldOffset(12)]
        public int Count;

        [FieldOffset(16)]
        public NonconvexContact Contact0;
        [FieldOffset(48)]
        public NonconvexContact Contact1;
        [FieldOffset(80)]
        public NonconvexContact Contact2;
        [FieldOffset(112)]
        public NonconvexContact Contact3;

        int IContactManifold<NonconvexContactManifold>.Count => Count;

        bool IContactManifold<NonconvexContactManifold>.Convex => false;

        /// <summary>
        /// 非凸流形中可以存在的最大接触数。
        /// </summary>
        public const int MaximumContactCount = 4;

        [Conditional("DEBUG")]
        private void ValidateIndex(int contactIndex)
        {
            Debug.Assert(contactIndex >= 0 && contactIndex < Count, "Contact index must be within the contact count.");
        }

        /// <summary>
        /// 检索联系人数据的副本。
        /// </summary>
        /// <param name="contactIndex">要从中复制数据的联系人的索引。</param>
        /// <param name="Offset">从第一个可碰撞对象的位置到接触位置的偏移量。</param>
        /// 请求接触处的接触面的<param name="Normal">法线。从可碰撞B到可碰撞A的点</param>
        /// <param name="Depth">请求联系人的穿透深度。</param>
        /// 请求的联系人的<param name="feature ureId">功能ID。
        /// 功能ID表示可碰撞对象的哪些部分形成了联系人,并可用于跨帧跟踪唯一联系人。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId)
        {
            ValidateIndex(contactIndex);
            ref var contact = ref Unsafe.Add(ref Contact0, contactIndex);
            offset = contact.Offset;
            normal = contact.Normal;
            depth = contact.Depth;
            featureId = contact.FeatureId;
        }
        /// <summary>
        /// 检索与请求的联系人关联的功能ID。
        /// </summary>
        /// <param name="contactIndex">要获取其功能ID的联系人的索引。</param>
        /// <return>请求的联系人的功能ID。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public int GetFeatureId(int contactIndex)
        {
            ValidateIndex(contactIndex);
            return Unsafe.Add(ref Contact0, contactIndex).FeatureId;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void FastRemoveAt(NonconvexContactManifold* manifold, int index)
        {
            --manifold->Count;
            if (index < manifold->Count)
            {
                var contacts = &manifold->Contact0;
                contacts[index] = contacts[manifold->Count];
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Add(NonconvexContactManifold* manifold, ref Vector3 normal, ref ConvexContact convexContact)
        {
            Debug.Assert(manifold->Count < MaximumContactCount);
            ref var targetContact = ref (&manifold->Contact0)[manifold->Count++];
            targetContact.Depth = convexContact.Depth;
            targetContact.Offset = convexContact.Offset;
            targetContact.Normal = normal;
            targetContact.FeatureId = convexContact.FeatureId;
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static ref NonconvexContact Allocate(NonconvexContactManifold* manifold)
        {
            Debug.Assert(manifold->Count < MaximumContactCount);
            return ref (&manifold->Contact0)[manifold->Count++];
        }

        /// <summary>
        /// 拉出对接触深度的引用。
        /// </summary>
        /// <param name="Manifold">要从中提取引用的流形。</param>
        /// <param name="contactIndex">要从中提取数据的联系人。</param>
        /// <return>对联系人深度的引用。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ref float GetDepth(ref NonconvexContactManifold manifold, int contactIndex)
        {
            return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Depth;
        }

        /// <summary>
        /// 拉出对联系人正常状态的引用。从可碰撞B到可碰撞A的点。
        /// </summary>
        /// <param name="Manifold">要从中提取引用的流形。</param>
        /// <param name="contactIndex">要从中提取数据的联系人。</param>
        /// <return>对联系人正常的引用。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ref Vector3 GetNormal(ref NonconvexContactManifold manifold, int contactIndex)
        {
            return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Normal;
        }


        /// <summary>
        /// 拉出对接触偏移量的引用。
        /// </summary>
        /// <param name="Manifold">要从中提取引用的流形。</param>
        /// <param name="contactIndex">要从中提取数据的联系人。</param>
        /// <return>对联系人偏移量的引用。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ref Vector3 GetOffset(ref NonconvexContactManifold manifold, int contactIndex)
        {
            return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Offset;
        }

        /// <summary>
        /// 拉出对联系人功能ID的引用。
        /// </summary>
        /// <param name="Manifold">要从中提取引用的流形。</param>
        /// <param name="contactIndex">要从中提取数据的联系人。</param>
        /// <return>对联系人功能ID的引用。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ref int GetFeatureId(ref NonconvexContactManifold manifold, int contactIndex)
        {
            return ref Unsafe.Add(ref manifold.Contact0, contactIndex).FeatureId;
        }
    }

    /// <summary>
    /// 包含与凸接触流形关联的数据。
    /// </summary>
    [StructLayout(LayoutKind.Explicit, Size = 108)]
    public unsafe struct ConvexContactManifold : IContactManifold<ConvexContactManifold>
    {
        /// <summary>
        /// 从可碰撞A到可碰撞B的偏移。
        /// </summary>
        [FieldOffset(0)]
        public Vector3 OffsetB;
        [FieldOffset(12)]
        public int Count;

        /// <summary>
        /// 所有触点共享的曲面法线。从可碰撞B到可碰撞A的点。
        /// </summary>
        [FieldOffset(16)]
        public Vector3 Normal;

        [FieldOffset(28)]
        public ConvexContact Contact0;
        [FieldOffset(48)]
        public ConvexContact Contact1;
        [FieldOffset(68)]
        public ConvexContact Contact2;
        [FieldOffset(88)]
        public ConvexContact Contact3;

        int IContactManifold<ConvexContactManifold>.Count => Count;

        bool IContactManifold<ConvexContactManifold>.Convex => true;

        [Conditional("DEBUG")]
        private void ValidateIndex(int contactIndex)
        {
            Debug.Assert(contactIndex >= 0 && contactIndex < Count, "Contact index must be within the contact count.");
        }

        /// <summary>
        /// 检索与请求的联系人关联的功能ID。
        /// </summary>
        /// <param name="contactIndex">要获取其功能ID的联系人的索引。</param>
        /// <return>请求的联系人的功能ID。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public int GetFeatureId(int contactIndex)
        {
            ValidateIndex(contactIndex);
            return Unsafe.Add(ref Contact0, contactIndex).FeatureId;
        }

        /// <summary>
        /// 检索联系人数据的副本。
        /// </summary>
        /// <param name="contactIndex">要从中复制数据的联系人的索引。</param>
        /// <param name="Offset">从第一个可碰撞对象的位置到接触位置的偏移量。</param>
        /// 请求接触处的接触面的<param name="Normal">法线。从可碰撞B到可碰撞A的点</param>
        /// <param name="Depth">请求联系人的穿透深度。</param>
        /// 请求的联系人的<param name="feature ureId">功能ID。
        /// 功能ID表示可碰撞对象的哪些部分形成了联系人,并可用于跨帧跟踪唯一联系人。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GetContact(int contactIndex, out Vector3 offset, out Vector3 normal, out float depth, out int featureId)
        {
            ValidateIndex(contactIndex);
            ref var contact = ref Unsafe.Add(ref Contact0, contactIndex);
            offset = contact.Offset;
            normal = Normal;
            depth = contact.Depth;
            featureId = contact.FeatureId;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void FastRemoveAt(ref ConvexContactManifold manifold, int index)
        {
            --manifold.Count;
            if (index < manifold.Count)
            {
                Unsafe.Add(ref manifold.Contact0, index) = Unsafe.Add(ref manifold.Contact0, manifold.Count);
            }
        }

        /// <summary>
        /// 拉出对接触深度的引用。
        /// </summary>
        /// <param name="Manifold">要从中提取引用的流形。</param>
        /// <param name="contactIndex">要从中提取数据的联系人。</param>
        /// <return>对联系人深度的引用。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ref float GetDepth(ref ConvexContactManifold manifold, int contactIndex)
        {
            return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Depth;
        }

        /// <summary>
        /// 拉出对接触歧管法线的引用。从可碰撞B到可碰撞A的点在所有接触之间共享单个法线。
        /// </summary>
        /// <param name="Manifold">要从中提取引用的流形。</param>
        /// <param name="contactIndex">要从中提取数据的联系人。</param>
        /// <return>对触点流形的正常引用。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ref Vector3 GetNormal(ref ConvexContactManifold manifold, int contactIndex)
        {
            return ref manifold.Normal;
        }


        /// <summary>
        /// 拉出对接触偏移量的引用。
        /// </summary>
        /// <param name="Manifold">要从中提取引用的流形。</param>
        /// <param name="contactIndex">要从中提取数据的联系人。</param>
        /// <return>对联系人偏移量的引用。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ref Vector3 GetOffset(ref ConvexContactManifold manifold, int contactIndex)
        {
            return ref Unsafe.Add(ref manifold.Contact0, contactIndex).Offset;
        }

        /// <summary>
        /// 拉出对联系人功能ID的引用。
        /// </summary>
        /// <param name="Manifold">要从中提取引用的流形。</param>
        /// <param name="contactIndex">要从中提取数据的联系人。</param>
        /// <return>对联系人功能ID的引用。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ref int GetFeatureId(ref ConvexContactManifold manifold, int contactIndex)
        {
            return ref Unsafe.Add(ref manifold.Contact0, contactIndex).FeatureId;
        }
    }

}
